#include <assert.h>
#include "bspline_curve.h"

namespace shermit {







#if enable_cuda
	//////////////////////////////////////////////////////////////////////
	//
	// implementation of Device BSplineCurve
	//
	//////////////////////////////////////////////////////////////////////
	template<typename float_t>
	GENERAL DCurve::DCurve() {
		control_pts = nullptr;
		num_control_pts = 0;
	}

	template<typename float_t>
	GENERAL inline const int DCurve::degree() const {
		return 3;
	}

	template<typename float_t>
	GENERAL inline const int DCurve::nControlPts() const {
		return num_control_pts;
	}

	template<typename float_t>
	GENERAL inline const int DCurve::nKnots() const {
		return nControlPts() + degree() + 1;
	}

	template<typename float_t>
	const DCurve::vec_t* DCurve::getControlPts() const {
		return control_pts;
	}

	template<typename float_t>
	GENERAL inline float_t DCurve::tMin() const {
		return (float_t)degree();
	}

	template<typename float_t>
	GENERAL inline float_t DCurve::tMax() const {
		return (float_t)nControlPts();
	}

	template<typename float_t>
	__device__ DCurve::vec_t DCurve::at(float_t t) const {
		int k = t;
		float_t t0 = t - (k - degree());
		float_t it0 = 4 - t0;
		float_t b0 = it0 * it0 * it0 / 6;
		float_t t1 = t0 - 1;
		float_t it1 = 4 - t1;
		float_t b1 = (-3 * it1 * it1 * it1 + 12 * it1 * it1 - 12 * it1 + 4) / 6;
		float_t t2 = t1 - 1;
		float_t b2 = (-3 * t2 * t2 * t2 + 12 * t2 * t2 - 12 * t2 + 4) / 6;
		float_t t3 = t2 - 1;
		float_t b3 = t3 * t3 * t3 / 6;

		// calculate the curve point
		return
			control_pts[k - 3] * b0 +
			control_pts[k - 2] * b1 +
			control_pts[k - 1] * b2 +
			control_pts[k - 0] * b3;
	}

	template<typename float_t>
	__device__ DCurve::vec_t DCurve::df(float_t t, uint order) const {
		switch (order) {
		case 0:
			return at(t);
		case 1:
			return df1(t);
		default:
			return df2(t);  // df3 not implemented
		}
	}

	template<typename float_t>
	__device__ inline DCurve::vec_t DCurve::df1(float_t t) const {
		int k = t;
		float_t t0 = t - (k - degree());
		float_t t0_2 = t0 * t0;
		float_t db0 = -(t0 - 4) * (t0 - 4) / 2;
		float_t db1 = (9 * t0_2 - 66 * t0 + 117) / 6;
		float_t db2 = (-9 * t0_2 + 60 * t0 - 96) / 6;
		float_t db3 = (t0 - 3) * (t0 - 3) / 2;

		return
			control_pts[k - 3] * db0 +
			control_pts[k - 2] * db1 +
			control_pts[k - 1] * db2 +
			control_pts[k - 0] * db3;
	}

	template<typename float_t>
	__device__ inline DCurve::vec_t DCurve::df2(float_t t) const {
		int k = t;
		float_t t0 = t - (k - degree());
		float_t d2_b0 = 4 - t0;
		float_t d2_b1 = 3 * t0 - 11;
		float_t d2_b2 = -3 * t0 + 10;
		float_t d2_b3 = t0 - 3;

		return
			control_pts[k - 3] * d2_b0 +
			control_pts[k - 2] * d2_b1 +
			control_pts[k - 1] * d2_b2 +
			control_pts[k - 0] * d2_b3;
	}

	template<typename float_t>
	__device__ DCurve::vec_t DCurve::tangent(float_t t) const {
		return nv::normalize(df1(t));
	}

	template<typename float_t>
	__device__ DCurve::vec_t DCurve::normal(float_t t) const {
		vec_t tgt = tangent(t);
		vec_t d2_p = df2(t);
		int sign = (tgt[0] * d2_p[1] - tgt[1] * d2_p[0]) > 0; // sign of curvature
		if (tgt.x * sign < 0) return vec_t(-tgt.y, tgt.x);
		return vec_t(tgt.y, -tgt.x);
	}

	template<typename float_t>
	__device__ const float_t DCurve::curvature(float_t t) const {
		vec_t d1_p = df1(t);
		vec_t d2_p = df2(t);
		float_t nmt = abs(d1_p[0] * d2_p[1] - d1_p[1] * d2_p[0]);
		float_t dmt = nv::length(d1_p);
		dmt = dmt * dmt * dmt;
		return nmt / dmt;
	}

	template<typename float_t>
	void DCurve::copyToHost(BSplineCurve<float_t, 3, DeviceType::CPU>& h_curve) const {
		h_curve.reset();
		h_curve.setControlPts(this->control_pts, this->num_control_pts, DeviceType::GPU);
	}

	template<typename float_t>
	void DCurve::setControlPts(const vec_t* pts, int num_pts, DeviceType from) {
		if (!pts || num_pts <= 0) return;
		reset();
		checkCudaErrors(GPUMalloc(control_pts, sizeof(vec_t) * num_pts));
		if (from == DeviceType::CPU) {
			checkCudaErrors(CPU2GPU(control_pts, pts, sizeof(vec_t) * num_pts));
		}
		else {
			checkCudaErrors(GPU2GPU(control_pts, pts, sizeof(vec_t) * num_pts));
		}
		return;
	}

	template<typename float_t>
	void DCurve::reset() {
		checkCudaErrors(cudaFree(control_pts));
		control_pts = nullptr;
		num_control_pts = 0;
	}
#endif // enable_cuda
}